#include "Spline.h"

#include <cmath>
#include <climits>
#include <cfloat>
#include <cassert>

using namespace Mesh;
using namespace Calculus;
using namespace LinearAlgebra;
using namespace UniPolynomial;

#define SIGN(x) ((x) > 0 ? 1 : -1)
#define MIN(a, b) ((a) > (b) ? (b) : (a))
#define ACCURACY 1e-8

Spline::Spline(int up, int vp) : m_u(up), m_v(vp), m_ctrlPoint(std::deque<point3f>(up * vp))
{
}

// 设置控制点
void Spline::setCtrlPoints(std::deque<point3f> pList)
{
	// 必须确保控制点数满足要求
	assert(m_ctrlPoint.size() == pList.size());
	m_ctrlPoint = pList;
}

// 获得控制点数组
std::deque<point3f> Spline::getCtrlPolygon() const
{
	return m_ctrlPoint;
}

// 输出曲线数组
void Spline::output(int udivide, double *array) const
{
	double us = paramUBegin();
	double ue = paramUEnd();

	// 计算并储存所有点的数据
	double step = (ue - us) / (udivide - 1);
	for (int i = 0; i < udivide; i++)
	{
		point3f p = calculateU(us + i * step);
		array[3 * i + 0] = p.x;
		array[3 * i + 1] = p.y;
		array[3 * i + 2] = p.z;
	}
}

// 输出网格数组
void Spline::output(int udivide, int vdivide, double *array) const
{
	double us = paramUBegin();
	double vs = paramVBegin();
	double ue = paramUEnd();
	double ve = paramVEnd();

	// 计算并储存所有点的数据
	double ustep = (ue - us) / (udivide - 1);
	double vstep = (ve - vs) / (vdivide - 1);
	for (int i = 0; i < udivide; i++)
	{
		for (int j = 0; j < vdivide; j++)
		{
			point3f p = calculateUV(us + i * ustep, vs + j * vstep);
			array[3 * vdivide * i + 3 * j + 0] = p.x;
			array[3 * vdivide * i + 3 * j + 1] = p.y;
			array[3 * vdivide * i + 3 * j + 2] = p.z;
		}
	}
}

// 输出控制点数组
void Spline::control(double *array) const
{
	// 横向绘制折线
	for (int i = 0; i < m_v; i++)
	{
		for (int j = 0; j < m_u; j++)
		{
			array[3 * (i * m_u + j) + 0] = m_ctrlPoint[i * m_u + j].x;
			array[3 * (i * m_u + j) + 1] = m_ctrlPoint[i * m_u + j].y;
			array[3 * (i * m_u + j) + 2] = m_ctrlPoint[i * m_u + j].z;
		}
	}
}

// 矩形网格的三角网格化
void Spline::toMesh(HEMesh *mesh, int udivide, int vdivide) const
{
	double us = paramUBegin();
	double vs = paramVBegin();
	double ue = paramUEnd();
	double ve = paramVEnd();

	// 计算并储存所有点的数据
	double ustep = (ue - us) / (udivide - 1);
	double vstep = (ve - vs) / (vdivide - 1);
	for (int i = 0; i < vdivide; i++)
		for (int j = 0; j < udivide; j++)
			mesh->CreateVertex(calculateUV(us + j * ustep, vs + i * vstep));

	// 推入三角形，注意法向，当应用光照时需要考虑
	// 这里向右向上为正，按照逆时针方向插入点
	// 因此控制点应当按照从左到右，从前到后的顺序排列才能保证法向
	// 即 z 从负向到正向，x 从负向到正向。在 OpenGL 中可能比较奇怪，因为 z 轴是朝向屏幕外的
	std::deque<vertex3f *> &vList = mesh->m_vList;
	for (int i = 0; i < vdivide - 1; i++)
	{
		for (int j = 0; j < udivide - 1; j++)
		{
			vertex3f *v0 = vList[i * udivide + j];
			vertex3f *v1 = vList[i * udivide + j + 1];
			vertex3f *v2 = vList[(i + 1) * udivide + j];
			vertex3f *v3 = vList[(i + 1) * udivide + j + 1];

			// 创建三角面
			mesh->CreateFace(v0, v2, v3, v1);
		}
	}
}

double Bezier::paramUBegin() const { return 0; }  // 获得 u 起始参数
double Bezier::paramUEnd() const { return 1; };	  // 获得 u 终点参数
double Bezier::paramVBegin() const { return 0; }; // 获得 v 起始参数
double Bezier::paramVEnd() const { return 1; };	  // 获得 v 终点参数
int Bezier::uDegree() const { return m_u - 1; };  // 获取 u 次数信息
int Bezier::vDegree() const { return m_v - 1; };  // 获取 v 次数信息

// 已知曲线上的点，反求参数 u
double Bezier::paramU(point3f p) const
{
	int n = uDegree();

	// 先计算曲线多项式，每个分量计算一个
	// 这里提前减去曲线点的分量
	Polynomial polyx({-p.x}), polyy({-p.y}), polyz({-p.z});

	// 计算 n+1 个控制点累计的多项式
	for (int i = 0; i < n + 1; i++)
	{
		Polynomial Bin = Bernstein(n, i);
		polyx = polyx + Bin * m_ctrlPoint[i].x;
		polyy = polyy + Bin * m_ctrlPoint[i].y;
		polyz = polyz + Bin * m_ctrlPoint[i].z;
	}

	FSolve fs;
	Vector x(1);

	// n 次 Bezier 曲线，每次降低次数
	for (int i = 0; i < n; i++)
	{
		// 选择参数起始位置
		x[0] = (paramUBegin() + paramUEnd()) / 2;

		// 建立三个方程，求根；
		FuncXX F = [=](Vector t) -> Vector
		{
			return {polyx(t[0]), polyy(t[0]), polyz(t[0])};
		};
		// 牛顿法求解
		fs(F, x);

		// 检查根是否在参数范围内，如果在就返回
		if (x[0] >= paramUBegin() && x[0] <= paramUEnd())
			return x[0];

		// 否则需要重新求解，对每个分量多项式去除这个根
		polyx = polyx / Polynomial({-x[0], 1});
		polyy = polyy / Polynomial({-x[0], 1});
		polyz = polyz / Polynomial({-x[0], 1});
	}

	return -1;
}

// 已知曲面上的点，反求参数 u,v
Vector Bezier::paramUV(point3f p) const
{
	assert(vDegree() > 0);

	int un = uDegree();
	int vn = vDegree();

	// 先计算基函数
	std::vector<Polynomial> uBin;
	std::vector<Polynomial> vBin;

	for (int i = 0; i < un + 1; i++)
		uBin.push_back(Bernstein(un, i));
	for (int i = 0; i < vn + 1; i++)
		vBin.push_back(Bernstein(vn, i));

	FSolve fs;
	Vector x(2);

	// 选择参数起始位置
	x[0] = (paramUBegin() + paramUEnd()) / 2;
	x[1] = (paramUBegin() + paramUEnd()) / 2;

	// 建立三个方程，求根；
	FuncXX F = [=](Vector t) -> Vector
	{
		Vector v(3);
		for (int i = 0; i < vn + 1; i++)
		{
			for (int j = 0; j < un + 1; j++)
			{
				v[0] += uBin[j](t[0]) * vBin[i](t[1]) * m_ctrlPoint[i * (un + 1) + j].x;
				v[1] += uBin[j](t[0]) * vBin[i](t[1]) * m_ctrlPoint[i * (un + 1) + j].y;
				v[2] += uBin[j](t[0]) * vBin[i](t[1]) * m_ctrlPoint[i * (un + 1) + j].z;
			}
		}
		// 注意减去要求解的点
		v[0] -= p.x;
		v[1] -= p.y;
		v[2] -= p.z;
		return v;
	};
	// 牛顿法求解
	fs(F, x);

	// 检查根是否在参数范围内，如果在就返回
	if (x[0] >= paramUBegin() && x[0] <= paramUEnd() && x[1] >= paramUBegin() && x[1] <= paramUEnd())
		return x;

	return Vector(2, -1);
}

// 获得 u 参数点
point3f Bezier::calculateU(double u) const
{
	std::deque<point3f> pList = m_ctrlPoint;
	return subdivide(pList, u);
}

// 获得 (u,v) 参数点
point3f Bezier::calculateUV(double u, double v) const
{
	assert(vDegree() > 0);
	std::deque<point3f> pList = m_ctrlPoint;
	return subdivide(pList, u, v);
}

// 曲线细分过程
point3f Bezier::subdivide(std::deque<point3f> &pList, double t) const
{
	// 如果只剩下一个点，细分结束
	if (pList.size() == 1)
		return pList.front();

	// 相邻两个点插值，删除最后一个点
	for (auto it = pList.begin(); (it + 1) != pList.end(); ++it)
	{
		(*it) = linear(*it, *(it + 1), t);
	}
	pList.pop_back();

	return subdivide(pList, t);
}

// 曲面细分过程
point3f Bezier::subdivide(std::deque<point3f> &pList, double u, double v) const
{
	// 如果只剩下一行点，行细分后结束
	if (pList.size() == m_u)
		return subdivide(pList, u);

	// 相邻两个列点插值
	int row = pList.size() / m_u;
	for (int i = 0; i < row - 1; i++)
	{
		for (int j = 0; j < m_u; j++)
		{
			// 纵向线性插值
			pList[i * m_u + j] = linear(
				pList[i * m_u + j],
				pList[(i + 1) * m_u + j],
				v);
		}
	}
	// 删除最后一行
	for (int j = 0; j < m_u; j++)
	{
		pList.pop_back();
	}

	return subdivide(pList, u, v);
}

double BSpline::paramUBegin() const { return m_uKnot[uDegree() - 1]; }			  // 获得 u 起始参数
double BSpline::paramUEnd() const { return m_uKnot[m_uKnot.size() - uDegree()]; } // 获得 u 终点参数
double BSpline::paramVBegin() const { return m_vKnot[vDegree() - 1]; }			  // 获得 v 起始参数
double BSpline::paramVEnd() const { return m_vKnot[m_vKnot.size() - vDegree()]; } // 获得 v 终点参数
int BSpline::uDegree() const { return m_uKnot.size() - m_u + 1; }				  // 获取 u 次数信息
int BSpline::vDegree() const { return m_vKnot.size() - m_v + 1; }				  // 获取 v 次数信息

// 已知曲线上的点，反求参数 u
double BSpline::paramU(point3f p) const
{
	// 需要寻找的控制点个数
	int order = uDegree() + 1;

	// 为了方便构造基函数，左右各添加一个节点
	std::deque<Real> uKnot;
	for (int i = 0; i < m_uKnot.size(); ++i)
		uKnot.push_back(m_uKnot[i]);

	uKnot.push_back(m_uKnot.back());
	uKnot.push_front(m_uKnot.front());

	// 找到相邻的控制点，使得其凸包包含 p
	for (int i = 0; i < m_ctrlPoint.size() - order + 1; i++)
	{
		// 首先获得相邻点的多边形
		std::deque<point3f> polygon;
		for (int j = 0; j < order; j++)
			polygon.push_back(m_ctrlPoint[i + j]);

		// 如果在内部，对于此凸包计算基函数
		if (inHull2D(p, polygon))
		{
			// 先计算曲线多项式，每个分量计算一个
			PiecewisePolynomial polyx, polyy, polyz;

			// 对每个控制点计算基函数
			for (int j = 0; j < order; j++)
			{
				// 获得需要的节点 = 控制点数 + 1
				std::deque<Real> knot;
				for (int k = 0; k < order + 1; k++)
					knot.push_back(uKnot[i + j + k]);

				// 计算基函数
				PiecewisePolynomial Bin = BasicBSpline(knot);
				polyx = merge(polyx, Bin * m_ctrlPoint[i + j].x);
				polyy = merge(polyy, Bin * m_ctrlPoint[i + j].y);
				polyz = merge(polyz, Bin * m_ctrlPoint[i + j].z);
			}

			// 最后减去 p 的位置
			polyx = polyx - Polynomial({p.x});
			polyy = polyy - Polynomial({p.y});
			polyz = polyz - Polynomial({p.z});

			FSolve fs;
			Vector x(1);

			Real left = uKnot[i + order - 1];
			Real right = uKnot[i + order];

			// 循环求根
			for (int j = 0; j < uDegree(); j++)
			{
				// 选择参数起始位置，选择中点，是为了防止边界连接点的不连续性导致的不可导，从而无法迭代求根
				x[0] = (left + right) / 2;

				// 建立三个方程，求根；
				FuncXX F = [=](Vector t) -> Vector
				{
					return {polyx(t[0]), polyy(t[0]), polyz(t[0])};
				};
				// 牛顿法求解
				fs(F, x);

				// 检查根是否在节点参数范围内，并且计算出的结果与真实值足够近，如果在就返回
				if (x[0] >= left && x[0] <= right && distance(p, calculateU(x[0])) < ACCURACY)
					return x[0];

				// 否则需要重新求解，对每个分量多项式去除这个根
				polyx = polyx / Polynomial({-x[0], 1});
				polyy = polyy / Polynomial({-x[0], 1});
				polyz = polyz / Polynomial({-x[0], 1});
			}
		}
	}

	return -1;
}

// 已知曲面上的点，反求参数 u,v
Vector BSpline::paramUV(point3f p) const
{
	assert(vDegree() > 0);

	// 需要寻找的控制点个数
	int uorder = uDegree() + 1;
	int vorder = vDegree() + 1;

	// 为了方便构造基函数，左右各添加一个节点
	std::deque<Real> uKnot;
	for (int i = 0; i < m_uKnot.size(); ++i)
		uKnot.push_back(m_uKnot[i]);

	uKnot.push_back(m_uKnot.back());
	uKnot.push_front(m_uKnot.front());

	// 为了方便构造基函数，左右各添加一个节点
	std::deque<Real> vKnot;
	for (int i = 0; i < m_vKnot.size(); ++i)
		vKnot.push_back(m_vKnot[i]);

	vKnot.push_back(m_vKnot.back());
	vKnot.push_front(m_vKnot.front());

	// 首先要计算出包含了点的凸包
	// 找到相邻的控制点，使得其凸包包含 p
	for (int i = 0; i < m_v - vorder + 1; i++)
	{
		for (int j = 0; j < m_u - uorder + 1; j++)
		{
			// 首先获得相邻点的凸包
			HEMesh mesh;
			for (int k = 0; k < vorder; k++)
				for (int l = 0; l < uorder; l++)
					mesh.CreateVertex(m_ctrlPoint[(i + k) * m_u + j + l]);

			// 计算凸包，如果控制点在凸包内部，计算基函数
			if (convexHull(&mesh) && mesh.IsInMesh(p))
			{
				std::vector<PiecewisePolynomial> uBin;
				std::vector<PiecewisePolynomial> vBin;

				// 对每个控制点计算基函数
				for (int k = 0; k < uorder; k++)
				{
					// 获得需要的节点 = 控制点数 + 1
					std::deque<Real> knot;
					for (int l = 0; l < uorder + 1; l++)
						knot.push_back(uKnot[j + k + l]);

					// 计算基函数
					uBin.push_back(BasicBSpline(knot));
				}
				// 对每个控制点计算基函数
				for (int k = 0; k < vorder; k++)
				{
					// 获得需要的节点 = 控制点数 + 1
					std::deque<Real> knot;
					for (int l = 0; l < vorder + 1; l++)
						knot.push_back(vKnot[i + k + l]);

					// 计算基函数
					vBin.push_back(BasicBSpline(knot));
				}

				// 建立三个方程，求根；
				FuncXX F = [=](Vector t) -> Vector
				{
					Vector v(3);
					for (int k = 0; k < vorder; k++)
					{
						for (int l = 0; l < uorder; l++)
						{
							v[0] += uBin[l](t[0]) * vBin[k](t[1]) * m_ctrlPoint[(i + k) * m_u + j + l].x;
							v[1] += uBin[l](t[0]) * vBin[k](t[1]) * m_ctrlPoint[(i + k) * m_u + j + l].y;
							v[2] += uBin[l](t[0]) * vBin[k](t[1]) * m_ctrlPoint[(i + k) * m_u + j + l].z;
						}
					}
					// 注意减去要求解的点
					v[0] -= p.x;
					v[1] -= p.y;
					v[2] -= p.z;
					return v;
				};

				// 准备构造求解
				FSolve fs;
				Vector x(2);

				// 获得参数范围
				Real uleft = uKnot[j + uorder - 1];
				Real uright = uKnot[j + uorder];
				Real vleft = vKnot[i + vorder - 1];
				Real vright = vKnot[i + vorder];

				// 选择参数起始位置
				x[0] = (uleft + uright) / 2;
				x[1] = (vleft + vright) / 2;

				// 牛顿法求解
				fs(F, x);

				// 检查根是否在节点参数范围内，并且与真实值足够近，如果在就返回
				if (x[0] >= uleft && x[0] <= uright && x[1] >= vleft && x[1] <= vright && distance(p, calculateUV(x[0], x[1])) < ACCURACY)
					return x;
			}
		}
	}

	return Vector(2, -1);
}

// 创建 u 均匀节点向量
void BSpline::uniformUKnot(int deg, double start, double end)
{
	int points = m_u;
	m_uKnot.clear();

	double step = (end - start) / (points - deg);

	for (int i = 0; i < deg; i++)
		m_uKnot.push_back(start);

	for (int i = 1; i < points - deg; i++)
		m_uKnot.push_back(start + i * step);

	for (int i = 0; i < deg; i++)
		m_uKnot.push_back(end);
}

// 创建 v 均匀节点向量
void BSpline::uniformVKnot(int deg, double start, double end)
{
	int points = m_v;
	m_vKnot.clear();

	double step = (end - start) / (points - deg);

	for (int i = 0; i < deg; i++)
		m_vKnot.push_back(start);

	for (int i = 1; i < points - deg; i++)
		m_vKnot.push_back(start + i * step);

	for (int i = 0; i < deg; i++)
		m_vKnot.push_back(end);
}

// 设置 u 节点向量
void BSpline::setUKnot(std::deque<double> v)
{
	m_uKnot = v;
}

// 设置 v 节点向量
void BSpline::setVKnot(std::deque<double> v)
{
	m_vKnot = v;
}

// 获得 u 参数点
point3f BSpline::calculateU(double u) const
{
	int deg = uDegree();

	// 初始化节点向量和控制点
	std::deque<double> kList;
	std::deque<point3f> pList;
	for (int i = 0; i < m_uKnot.size(); i++)
	{
		if (m_uKnot[i + deg] >= u)
		{
			// 节点数是次数的两倍
			for (int j = 0; j < deg * 2; j++)
			{
				kList.push_back(m_uKnot[i + j]);
			}
			// 控制点数是次数加一
			for (int j = 0; j < deg + 1; j++)
			{
				pList.push_back(m_ctrlPoint[i + j]);
			}
			break;
		}
	}

	return subdivide(kList, pList, u);
}

// 获得 (u,v) 参数点
point3f BSpline::calculateUV(double u, double v) const
{
	int uDeg = uDegree();
	int vDeg = vDegree();

	int uindex = 0;
	int vindex = 0;

	// 初始化节点向量和控制点
	std::deque<double> ukList;
	std::deque<double> vkList;
	for (int i = 0; i < m_uKnot.size(); i++)
	{
		if (m_uKnot[i + uDeg] >= u)
		{
			uindex = i;
			// 节点数是次数的两倍
			for (int j = 0; j < uDeg * 2; j++)
			{
				ukList.push_back(m_uKnot[i + j]);
			}
			break;
		}
	}
	for (int i = 0; i < m_vKnot.size(); i++)
	{
		if (m_vKnot[i + vDeg] >= v)
		{
			vindex = i;
			// 节点数是次数的两倍
			for (int j = 0; j < vDeg * 2; j++)
			{
				vkList.push_back(m_vKnot[i + j]);
			}
			break;
		}
	}

	std::deque<point3f> pList;
	// 控制点数是次数加一
	for (int i = vindex; i < vindex + vDeg + 1; i++)
	{
		for (int j = uindex; j < uindex + uDeg + 1; j++)
		{
			pList.push_back(m_ctrlPoint[i * m_u + j]);
		}
	}

	return subdivide(ukList, vkList, pList, u, v);
}

// 曲线细分过程
point3f BSpline::subdivide(std::deque<double> &kList, std::deque<point3f> &pList, double t) const
{
	// 如果只剩下一个点，细分结束
	if (pList.size() == 1)
		return pList.front();

	int deg = uDegree();			  // 获得次数
	int col = kList.size() - deg + 1; // u 控制点数

	// 相邻两个点插值，移动节点
	for (int j = 0; j < col - 1; j++)
	{
		pList[j] = linear(pList[j], pList[j + 1], t, kList[j], kList[j + deg]);
		kList[j] = kList[j + 1];
	}
	// 此时 kit 与 pit 移动到最后一个控制点位置

	// 节点插入位置是最后一个控制点位置的前一个，删除最后一个节点和最后一个控制点
	kList[col - 1] = kList[col - 2];
	kList[col - 2] = t;
	kList.pop_back();
	pList.pop_back();

	return subdivide(kList, pList, t);
}

point3f BSpline::subdivide(std::deque<double> &ukList, std::deque<double> &vkList, std::deque<point3f> &pList, double u, double v) const
{
	// 如果只剩下一行点，行细分后结束（只剩一行的时候 v 节点数等于次数）
	if (vkList.size() == vDegree())
		return subdivide(ukList, pList, u);

	int row = vkList.size() - vDegree() + 1; // v 控制点数
	int col = ukList.size() - uDegree() + 1; // u 控制点数

	// 相邻两个列点插值
	for (int i = 0; i < row - 1; i++)
	{
		for (int j = 0; j < col; j++)
		{
			// 纵向线性插值
			pList[i * col + j] = linear(
				pList[i * col + j],
				pList[(i + 1) * col + j],
				v,
				vkList[i],
				vkList[i + vDegree()]);
		}
		// 移动节点
		vkList[i] = vkList[i + 1];
	}

	// 节点插入位置是最后一个控制点位置的前一个，删除最后一个节点和最后一个控制点
	vkList[row - 1] = vkList[row - 2];
	vkList[row - 2] = v;
	vkList.pop_back();

	// 删除最后一行
	for (int j = 0; j < col; j++)
		pList.pop_back();

	return subdivide(ukList, vkList, pList, u, v);
}

// 计算并返回平面多边形的凸包
Polygon Mesh::convexHull2D(Polygon &polygon)
{
	// 按照顺序检查相邻两个边向量的叉积的符号，如果出现符号反转，说明两个边的公共顶点“凹陷“，将异号的顶点移除后得到凸包
	Polygon hull = polygon;

	// 三角形一定是凸包，作为退出条件
	while (hull.size() > 3)
	{
		int n = hull.size();

		std::deque<vector3f> pro(n);
		for (int i = 0; i < n; i++)
		{
			// 获得前后两个顶点的索引
			int indp1 = (i - 1 + n) % n;
			int indp2 = (i + 1) % n;
			vector3f p1 = hull[indp1] - hull[i];
			vector3f p2 = hull[i] - hull[indp2];

			// 保存叉积结果
			pro[i] = cross(p1, p2);
		}

		// 检查这些叉积的方向，同时统计符号
		int pos = 1;
		std::deque<int> index(n);
		for (int i = 1; i < n; i++)
		{
			// 全与第一个叉积做点积获得符号
			index[i] = SIGN(dot(pro[i], pro[0]));
			if (index[i] > 0)
				pos++;
		}

		// 如果全部同号，说明已经形成凸包
		if (pos == n)
			break;

		// 删除对应少数的符号
		int sign = (pos > (n - pos)) ? -1 : 1;
		auto it = hull.begin();
		for (int i = 0; i < n; i++)
		{
			if (index[i] == sign)
				it = hull.erase(it);
			else
				it++;
		}
	}

	return hull;
}

// 检查点是否在平面多边形的凸包内部
bool Mesh::inHull2D(point3f p, Polygon &polygon)
{
	// 首先计算凸包
	Polygon hull = convexHull2D(polygon);

	// 利用二分角检测
	int n = hull.size();
	int a = 1, b = n - 1;
	vector3f p0 = p - hull[0];

	// 检查点的顺序是逆时针还是顺时针
	vector3f pa = hull[a] - hull[0];
	vector3f pb = hull[b] - hull[0];

	// 叉积规范化作为法向
	vector3f normal = normalize(cross(pa, pb));

	// 行列式大于零说明是逆时针，小于零就交换端点
	if (det(pa, pb, normal) < 0)
	{
		int tmp = a;
		a = b;
		b = tmp;
	}

	// 循环直到两边界点相邻
	while (abs(a - b) > 1)
	{
		// 获得等分点与初始顶点的向量
		int mid = (a + b) / 2;
		vector3f m0 = hull[mid] - hull[0];
		double sign = det(p0, m0, normal);

		// 判断如果恰好在连线上，就计算距离，如果距离满足直线关系，就说明在内部
		if (fabs(sign) < ACCURACY)
		{
			if (fabs(distance(hull[0], p) + distance(hull[mid], p) - distance(hull[0], hull[mid])) < ACCURACY)
				return true;
			else
				return false;
		}

		// 检查点在等分线的哪一侧，同时划分多边形
		if (det > 0)
			b = mid;
		else
			a = mid;
	}

	// 剩下的一定是三角形，只要判断是否在三角形内部
	vector3f p0pa = cross(p0, hull[a] - p);
	vector3f papb = cross(hull[a] - p, hull[b] - p);
	vector3f pbp0 = cross(hull[b] - p, hull[a] - p);

	// 如果向量方向不同，说明在外部
	if (SIGN(dot(p0pa, papb)) != SIGN(dot(papb, pbp0)))
		return false;

	return true;
}